/*
 * stepper.cpp
 *
 *  Created on: 7-Jul-2009
 *      Author: nrqm
 */

#include "../arduino/WProgram.h"
#include "stepper.h"

/*
 * The Arduino pins on which output patterns are sent to the stepper motor driver
 */

#define EN0		14		// The documentation says to combine the enable lines on one output
#define EN1		15		// pin, but I've had trouble doing that with other 3.3V boards.
#define L1		16
#define L2		17
#define L3      18
#define L4      19

#define CYCLE_DELAY_MS 20		// The delay between cycle transitions, in milliseconds
#define PITCH_STEP 0x800		// The difference in pitch bend value between positions
#define DEAD_ZONE 0x40

extern void print(const char* str);

extern uint16_t millis16();

Stepper::Stepper() :
	MidiPitchBendListener(),
	state(CYCLE1),
	max_positions(4),
	current_position(0),
	destination_position(0),
	current_cycle_start_time(0),
	current_pitch(0x2000)
{
	pinMode(EN0, OUTPUT);
	pinMode(EN1, OUTPUT);
	pinMode(L1, OUTPUT);
	pinMode(L2, OUTPUT);
	pinMode(L3, OUTPUT);
	pinMode(L4, OUTPUT);
	OutputCycleCode();
}

Stepper* Stepper::GetInstance()
{
	static Stepper instance = Stepper();
	return &instance;
}

void Stepper::TransitionToState(STEPPER_STATE new_state)
{
	// Output the bit pattern for the new cycle to the stepper motor driver.  The 6 least-significant bits
	// are set to 0 so that the new state can be masked in (the MSbs are left unchanged).
	state = new_state;
	OutputCycleCode();
	current_cycle_start_time = millis16();
}

void Stepper::OutputCycleCode()
{
	switch (state)
	{
	case CYCLE1:
		digitalWrite(EN0, HIGH);
		digitalWrite(EN1, LOW);
		digitalWrite(L1, HIGH);
		digitalWrite(L2, LOW);
		digitalWrite(L3, LOW);
		digitalWrite(L4, LOW);
		break;
	case CYCLE2:
		digitalWrite(EN0, HIGH);
		digitalWrite(EN1, LOW);
		digitalWrite(L1, LOW);
		digitalWrite(L2, HIGH);
		digitalWrite(L3, LOW);
		digitalWrite(L4, LOW);
		break;
	case CYCLE3:
		digitalWrite(EN0, LOW);
		digitalWrite(EN1, HIGH);
		digitalWrite(L1, LOW);
		digitalWrite(L2, LOW);
		digitalWrite(L3, HIGH);
		digitalWrite(L4, LOW);
		break;
	case CYCLE4:
		digitalWrite(EN0, LOW);
		digitalWrite(EN1, HIGH);
		digitalWrite(L1, LOW);
		digitalWrite(L2, LOW);
		digitalWrite(L3, LOW);
		digitalWrite(L4, HIGH);
		break;
	}
}

void Stepper::CycleCCW()
{
	switch (state)
	{
	case CYCLE4:
		TransitionToState(CYCLE2);
		break;
	case CYCLE2:
		TransitionToState(CYCLE3);
		break;
	case CYCLE3:
		TransitionToState(CYCLE1);
		break;
	case CYCLE1:
		TransitionToState(CYCLE4);
		current_position--;			// Step has completed.
		break;
	default:
		break;
	}
}

void Stepper::CycleCW()
{
	switch (state)
	{
	case CYCLE1:
		TransitionToState(CYCLE3);
		break;
	case CYCLE3:
		TransitionToState(CYCLE2);
		break;
	case CYCLE2:
		TransitionToState(CYCLE4);
		break;
	case CYCLE4:
		TransitionToState(CYCLE1);
		current_position++;			// Step has completed
		break;
	default:
		break;
	}
}

void Stepper::DoPitchBend(int16_t new_pitch)
{
	int16_t old_pitch = current_pitch;
	int8_t pitch_diff = 0;

	if (new_pitch < current_pitch - DEAD_ZONE)
	{
		// move a step or more to the left
		current_pitch -= ((current_pitch - new_pitch)/PITCH_STEP + 1) * PITCH_STEP;
	}
	else if (new_pitch > current_pitch + PITCH_STEP + DEAD_ZONE)
	{
		// move a step or more to the right
		// multiplying by PITCH_STEP/PITCH_STEP masks off some bits to make current_pitch an integer
		// multiple of PITCH_STEP (in this case 1 is not the multiplicative identity!!!?!?!?!)
		current_pitch += ((new_pitch - current_pitch) / PITCH_STEP) * PITCH_STEP;
	}
	else
	{
		return;
	}

	if (current_pitch < 0x2000)
	{
		// don't allow it to go left past the centre position.
		current_pitch = 0x2000;
		destination_position = 0;
		return;
	}

	if (old_pitch != current_pitch)
	{
		// calculate the new position.
		pitch_diff = (current_pitch - old_pitch) / PITCH_STEP;
		if (current_position + pitch_diff > max_positions)
		{
			// integer underflow case
			destination_position = 0;
		}
		else
		{
			destination_position += pitch_diff;
		}
	}
}

void Stepper::CheckStepper()
{
	uint16_t now = millis16();
	if (now - current_cycle_start_time > CYCLE_DELAY_MS)
	{
		// If the cycle timer has expired, then move to the next cycle.
		if (current_position < destination_position)
		{
			// Then the stick is to the left of its destination, and needs to move clockwise.
			CycleCW();
		}
		else if (current_position > destination_position)
		{
			// Then the stick is to the right of its destination, and needs to move counterclockwise.
			CycleCCW();
		}
		else
		{
			// It's all good, the stepper's already there!
		}
	}
}
